﻿/*
 * ©2009-2010 XELF
 * http://xelf.info
 */

#if XNA || WINDOWS_PHONE
using Microsoft.Xna.Framework;
#endif

namespace XELF.Framework {

	/// <summary>
	/// プリュッカー線座標 {U : V}
	/// </summary>
	public partial struct PluckerLine {
		/// <summary>
		/// U
		/// </summary>
		public Vector3 U;
		/// <summary>
		/// V
		/// </summary>
		public Vector3 V;

		/// <summary>
		/// コンストラクタ
		/// </summary>
		/// <param name="u"></param>
		/// <param name="v"></param>
		public PluckerLine(Vector3 u, Vector3 v) {
			U = u; V = v;
		}

		/// <summary>
		/// 文字列表現を得る。
		/// </summary>
		/// <returns></returns>
		public override string ToString() {
			return string.Format("[{0} : {1} : {2} : {3} : {4} : {5}]",
				U.X, U.Y, U.Z, V.X, V.Y, V.Z);
		}

		/// <summary>
		/// 2点を通る有向直線。{P - Q : P x Q}
		/// </summary>
		/// <param name="from"></param>
		/// <param name="to"></param>
		public static PluckerLine CreateFromPointToPoint(Vector3 from, Vector3 to) {
			var result = new PluckerLine();
			Vector3.Subtract(ref to, ref from, out result.U);
			Vector3.Cross(ref to, ref from, out result.V);
			return result;
		}

		/// <summary>
		/// 2点を通る有向直線。{qP - pQ : P x Q}
		/// </summary>
		/// <param name="from"></param>
		/// <param name="to"></param>
		public static PluckerLine CreateFromPointToPoint(Vector4 from, Vector4 to) {
			var result = new PluckerLine();
			result.U.X = from.W * to.X - to.W * from.X;
			result.U.Y = from.W * to.Y - to.W * from.Y;
			result.U.Z = from.W * to.Z - to.W * from.Z;
			result.V.X = to.Y * from.Z - to.Z * from.Y;
			result.V.Y = to.Z * from.X - to.X * from.Z;
			result.V.Z = to.X * from.Y - to.Y * from.X;
			return result;
		}

		/// <summary>
		/// 点と方向から有向直線。{U : U x Q}
		/// </summary>
		/// <param name="from"></param>
		/// <param name="direction"></param>
		public static PluckerLine CreateFromPointDirection(Vector3 from, Vector3 direction) {
			var result = new PluckerLine { U = direction, };
			Vector3.Cross(ref direction, ref from, out result.V);
			return result;
		}

		/// <summary>
		/// 直線上の点。(V x U + t U : U . U)
		/// </summary>
		/// <param name="t">パラメータ</param>
		/// <param name="point">位置ベクトル</param>
		public void GetPoint(float t, out Vector4 point) {
			Vector3 a, b;
			Vector3.Cross(ref V, ref U, out a);
			Vector3.Multiply(ref U, t, out b);
			Vector3.Add(ref a, ref b, out a);
			point = new Vector4(a, U.LengthSquared());
		}

		/// <summary>
		/// 直線上の点。(V x U + t U : U . U)
		/// </summary>
		/// <param name="t">パラメータ</param>
		/// <returns>位置ベクトル</returns>
		public Vector4 this[float t] {
			get {
				Vector4 result;
				GetPoint(t, out result);
				return result;
			}
		}

		/// <summary>
		/// 有向直線の位置関係を調べる。aU . bV + aV . bU
		/// </summary>
		/// <param name="a"></param>
		/// <param name="b"></param>
		/// <returns>0のとき交差か平行。0より大きいとき反時計回り。0未満のとき時計回り。</returns>
		public static float Side(ref PluckerLine a, ref PluckerLine b) {
			return
				a.U.X * b.V.X +
				a.U.Y * b.V.Y +
				a.U.Z * b.V.Z +
				a.V.X * b.U.X +
				a.V.Y * b.U.Y +
				a.V.Z * b.U.Z;
		}

		/// <summary>
		/// 有向直線の位置関係を調べる。aU . bV + aV . bU
		/// </summary>
		/// <param name="a"></param>
		/// <param name="b"></param>
		/// <param name="result">0のとき交差か平行。0より大きいとき反時計回り。0未満のとき時計回り。</param>
		public static void Side(ref PluckerLine a, ref PluckerLine b, out float result) {
			result =
				a.U.X * b.V.X +
				a.U.Y * b.V.Y +
				a.U.Z * b.V.Z +
				a.V.X * b.U.X +
				a.V.Y * b.U.Y +
				a.V.Z * b.U.Z;
		}

		/// <summary>
		/// 有向直線の位置関係を調べる。aU . bV + aV . bU
		/// </summary>
		/// <param name="a"></param>
		/// <param name="b"></param>
		/// <returns>0のとき交差か平行。0より大きいとき反時計回り。0未満のとき時計回り。</returns>
		public static float Side(PluckerLine a, PluckerLine b) {
			return
				a.U.X * b.V.X +
				a.U.Y * b.V.Y +
				a.U.Z * b.V.Z +
				a.V.X * b.U.X +
				a.V.Y * b.U.Y +
				a.V.Z * b.U.Z;
		}

		/// <summary>
		/// 有向直線aと点pを通るX軸(1,0,0)方向の有向直線との位置関係を調べる。
		/// </summary>
		/// <param name="a"></param>
		/// <param name="p"></param>
		/// <param name="result"></param>
		public static void SideX(ref PluckerLine a, ref Vector3 p, out float result) {
			result = a.V.X - a.U.Y * p.Z + a.U.Z * p.Y;
		}

		/// <summary>
		/// 有向直線aと点(x,y,z)を通るX軸(1,0,0)方向の有向直線との位置関係を調べる。
		/// </summary>
		/// <param name="a"></param>
		/// <param name="y"></param>
		/// <param name="z"></param>
		/// <param name="result"></param>
		public static void SideX(ref PluckerLine a, float y, float z, out float result) {
			result = a.V.X - a.U.Y * z + a.U.Z * y;
		}

		/// <summary>
		/// 有向直線aと点pを通るY軸(0,1,0)方向の有向直線との位置関係を調べる。
		/// </summary>
		/// <param name="a"></param>
		/// <param name="p"></param>
		/// <param name="result"></param>
		public static void SideY(ref PluckerLine a, ref Vector3 p, out float result) {
			result = a.V.Y + a.U.X * p.Z - a.U.Z * p.X;
		}

		/// <summary>
		/// 有向直線aと点(x,y,z)を通るY軸(0,1,0)方向の有向直線との位置関係を調べる。
		/// </summary>
		/// <param name="a"></param>
		/// <param name="x"></param>
		/// <param name="z"></param>
		/// <param name="result"></param>
		public static void SideY(ref PluckerLine a, float x, float z, out float result) {
			result = a.V.Y + a.U.X * z - a.U.Z * x;
		}

		/// <summary>
		/// 有向直線aと点pを通るZ軸(0,0,1)方向の有向直線との位置関係を調べる。
		/// </summary>
		/// <param name="a"></param>
		/// <param name="p"></param>
		/// <param name="result"></param>
		public static void SideZ(ref PluckerLine a, ref Vector3 p, out float result) {
			result = a.V.Z - a.U.X * p.Y + a.U.Y * p.X;
		}

		/// <summary>
		/// 有向直線aと点(x,y,z)を通るZ軸(0,0,1)方向の有向直線との位置関係を調べる。
		/// </summary>
		/// <param name="a"></param>
		/// <param name="x"></param>
		/// <param name="y"></param>
		/// <param name="result"></param>
		public static void SideZ(ref PluckerLine a, float x, float y, out float result) {
			result = a.V.Z - a.U.X * y + a.U.Y * x;
		}

		/// <summary>
		/// 平面と線の交点。(V x N - d U : U . N)
		/// </summary>
		/// <param name="plane"></param>
		/// <param name="line"></param>
		/// <param name="result"></param>
		public static void Intersects(ref Plane plane, ref PluckerLine line, out Vector4 result) {
			Vector3 v;
			float w;
			Vector3.Cross(ref line.V, ref plane.Normal, out v);
			Vector3 a;
			Vector3.Multiply(ref line.U, plane.D, out a);
			Vector3.Subtract(ref v, ref a, out v);
			Vector3.Dot(ref line.U, ref plane.Normal, out w);
			result = new Vector4(v.X, v.Y, v.Z, w);
		}

		/// <summary>
		/// 平面と線の交点。(V x N - d U : U . N)
		/// </summary>
		/// <param name="plane"></param>
		/// <param name="line"></param>
		/// <param name="result"></param>
		public static void Intersects(ref Plane plane, ref PluckerLine line, out Vector3 result) {
			Vector3 v;
			float w;
			Vector3.Cross(ref line.V, ref plane.Normal, out v);
			Vector3 a;
			Vector3.Multiply(ref line.U, plane.D, out a);
			Vector3.Subtract(ref v, ref a, out v);
			Vector3.Dot(ref line.U, ref plane.Normal, out w);
			var r = MathHelper.Rcp(w);
			result = new Vector3(v.X * r, v.Y * r, v.Z * r);
		}

		/// <summary>
		/// 反射方向ベクトル。-2 (N . U) N + U
		/// </summary>
		/// <param name="line">入射方向</param>
		/// <param name="plane">面</param>
		/// <param name="result"></param>
		public static void Reflect(ref PluckerLine line, ref Plane plane, out Vector3 result) {
			Vector3.Reflect(ref line.U, ref plane.Normal, out result);
		}

		/// <summary>
		/// 平面と平面の交差する線。{N1 x N2 : d2 N1 - d1 N2}
		/// </summary>
		/// <param name="planeA"></param>
		/// <param name="planeB"></param>
		/// <param name="result"></param>
		public static void Intersects(ref Plane planeA, ref Plane planeB, out PluckerLine result) {
			result = new PluckerLine();
			Vector3.Cross(ref planeA.Normal, ref planeB.Normal, out result.U);
			Vector3.Multiply(ref planeA.Normal, planeB.D, out result.V);
			Vector3 a;
			Vector3.Multiply(ref planeB.Normal, planeA.D, out a);
			Vector3.Subtract(ref result.V, ref a, out result.V);
		}

		/// <summary>
		/// 3直線で表された三角形と光線の交点の正規化されていない重心座標を得る。
		/// </summary>
		/// <param name="bc">有向直線bc</param>
		/// <param name="ca">有向直線ca</param>
		/// <param name="ab">有向直線ab</param>
		/// <param name="ray">光線</param>
		/// <param name="result">結果の正規化されていない重心座標</param>
		public static void Intersects(ref PluckerLine bc, ref PluckerLine ca, ref PluckerLine ab,
			ref PluckerLine ray, out Vector3 result) {
			result = new Vector3();
			Side(ref bc, ref ray, out result.X);
			Side(ref ca, ref ray, out result.Y);
			Side(ref ab, ref ray, out result.Z);
		}

		/// <summary>
		/// 三角形と光線の交点の正規化されていない重心座標と、交点を得る。
		/// </summary>
		/// <param name="a">頂点a</param>
		/// <param name="b">頂点b</param>
		/// <param name="c">頂点c</param>
		/// <param name="bc">有向直線bc</param>
		/// <param name="ca">有向直線ca</param>
		/// <param name="ab">有向直線ab</param>
		/// <param name="ray">光線</param>
		/// <param name="resultBarycentric">結果の正規化されていない重心座標</param>
		/// <param name="resultPoint">結果の交点の同次座標</param>
		public static void Intersects(
			ref Vector3 a, ref Vector3 b, ref Vector3 c,
			ref PluckerLine bc, ref PluckerLine ca, ref PluckerLine ab,
			ref PluckerLine ray,
			out Vector3 resultBarycentric, out Vector4 resultPoint) {
			Intersects(ref bc, ref ca, ref ab, ref ray, out resultBarycentric);
			Vector3 point;
			Vector3.Multiply(ref a, resultBarycentric.X, out point);
			Vector3Helper.Madd(ref b, resultBarycentric.Y, ref point, out point);
			Vector3Helper.Madd(ref c, resultBarycentric.Z, ref point, out point);
			resultPoint = new Vector4(
				point,
				resultBarycentric.X + resultBarycentric.Y + resultBarycentric.Z);
		}
	}

}